#!/usr/bin/env python
# -*-coding:utf-8 -*-

__all__ = ['SimWorld', 'getRobots', 'getMap', 'generateRandTask', 'getTimer', 'run']

import logging
import configparser
import numpy as np

from task import Task
from composite_task import CompositeTask
from map import Map
from communication_channel import CommunicationChannel
from robot import Robot
from robot_pool import RobotPool
from transportation_control_system import TransportationControlSystem
from traj import Traj
from timer import Timer
from obstacle import Obstacle
from obstacle_pool import ObstaclePool

class SimWorld(object):
    """
    虚拟“世界”，其中包含了所有的对象，包括机器人、调度器等等。
    """
    def __init__(self, cfg_file, **arg):
        """
        创建一个虚拟世界。
        :param cfg_file: 此“世界”的配置文件名。根据该配置文件创建虚拟世界。配置文件模板见根目录下的config.ini文件。
        """
        self.initLogger()
        cfg = configparser.ConfigParser()
        cfg.read(cfg_file)

        map_file = cfg['Map']['file']
        self._dt = cfg['SimWorld'].getfloat('dt')
        
        if 'robot_num' in arg:
            self._robot_num = arg['robot_num']
        else:
            self._robot_num = cfg['Robots'].getint('number')
            
        if 'task_num' in arg:
            self._task_num = arg['task_num']
        else:
            self._task_num = 10
        
        self._map = Map(map_file)
        self._timer = Timer()

        self._comm_channel = CommunicationChannel(self._timer)
        self._robot_pool = RobotPool()
        self._obstacle_pool = ObstaclePool()
        
        np.random.seed(None)
        leaf_nodes = self._map.getLeafNodes()
        waypoint = np.random.choice(leaf_nodes, size=self._robot_num, replace=False)
        #以下为测试用例，用于调试程序。需与addRandomTasks函数中对应的dst赋值语句配合使用。
        #waypoint = ['46', '33', '3', '36', '45', '48', '21', '52']
        print('initial positions', waypoint)
        self._logger.info('Initial positions of robots are {0}'.format(waypoint))
        for i in range(self._robot_num):
            self._robot_pool.add(Robot(self, _id=i, waypoint=waypoint[i]))
            
        self._tcs = TransportationControlSystem(self)
        self._traj = Traj(self._robot_num)
        
    def initLogger(self):
        level = logging.DEBUG
        # create logger
        self._logger = logging.getLogger('MyTCS')
        self._logger.setLevel(level)

        # create file handler and set level
        fh = logging.FileHandler(filename='log.txt')
        fh.setLevel(level)

        # create formatter
        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')

        # add formatter to fh
        fh.setFormatter(formatter)

        # add fh to logger
        self._logger.addHandler(fh)

    def getRobots(self):
        """
        获取机器人列表。
        :return: 所有机器人的一个列表（list)。
        """
        return self._robot_pool.getRobots()
        
    def getRobotPool(self):
        return self._robot_pool

    def getMap(self):
        """
        获取地图。
        :return: 地图（Map对象）。
        """
        return self._map
        
    def getTimer(self):
        """
        获取计时器。
        :return: 计时器（Timer对象）。
        """
        return self._timer
        
    def getTCS(self):
        return self._tcs

    def addRandomTasks(self, num):
        """
        产生一个随机任务，该任务的起始节点和目的节点都是随机产生的。
        :return: 所产生的任务。
        """
        leaf_nodes = self._map.getLeafNodes()
        src = np.random.choice(leaf_nodes, size=num, replace=True)
        dst = np.random.choice(leaf_nodes, size=num, replace=True)
        #以下为测试用例，用于调试程序。需与__init__函数中对应的waypoint赋值语句配合使用。
        #dst = ['45', '20', '49', '52', '48', '31', '45', '45', '21', '1' , '46', '42', '49', '3' , '49',
        #       '1' , '38', '52', '3' , '42', '3' , '20', '42', '38', '21', '31', '31', '31', '49', '51',
        #       '33', '43', '36', '49', '36', '3' , '51', '38', '48', '52', '43', '1' , '31', '38', '38',
        #       '43', '45', '45', '36', '33', '3' , '48', '38', '21', '21', '21', '43', '48', '3' , '52',
        #       '3' , '42', '52', '36', '3' , '36', '52', '1' , '48', '45', '1' , '49', '43', '52', '21',
        #       '31', '31', '1' , '31', '51', '21', '33', '36', '1' , '43', '1' , '20', '21', '51', '1' ,
        #       '42', '42', '33', '51', '1' , '52', '36', '33', '20', '36', '46', '46', '49', '38', '48',
        #       '33', '46', '48', '43', '21', '1' , '1' , '21', '45', '20', '49', '21', '38', '46', '51',
        #       '31', '20', '49', '45', '33', '52', '43', '31', '3' , '42', '42', '49', '20', '21', '21',
        #       '49', '1' , '42', '3' , '38', '33', '36', '46', '43', '1' , '51', '36', '20', '52', '48']
        self._logger.info('Created tasks with destination nodes {0}'.format(dst))
        simulation_time = 300
        due = simulation_time/10
        delay = np.random.random_sample(size=num) * simulation_time
        #delay = [320.81394495, 119.8577101 , 241.11191472, 346.69048568, 113.7529524 ,
        #         337.38751034,  35.98038769,  16.54651655, 187.27422331, 227.66185797,
        #          93.86864757, 200.61157909, 308.71122708, 163.7360935 , 304.56643615,
        #         110.94355949, 156.8907221 ,  36.66755452,  82.9967003 ,  77.81456214,
        #         133.66904482, 133.39593737, 182.0071855 , 171.67050569, 287.17578911,
        #         151.83089216, 329.326443  , 113.14970715, 349.28904645, 125.76568922,
        #         200.23248643, 327.3013753 ,  98.08329993, 232.51523791, 173.35327653,
        #          44.22338677, 204.34821956,  94.87603679, 171.66164807, 112.66553711,
        #          28.06896323, 124.36048225, 209.25298283, 191.38718345, 264.75289003,
        #         120.18266696, 132.04878555,  56.42424303,  42.43867607,  45.4853961 ,
        #         151.55207735,  25.79279483, 129.78987105,  51.24717649,  22.67923728,
        #         255.33336364, 342.40146418, 305.85068283, 158.76515006,  20.13752995,
        #          73.56406993, 247.47750373, 334.87113374, 282.29626826, 167.09445976,
        #          65.04935093,  47.73489916,  98.39704766, 168.76146476, 330.33579972,
        #          36.64435929, 156.84268968, 350.72018657, 247.57673775, 237.53814979,
        #         130.67439272, 236.20765921,  49.09205641, 118.37993975, 171.80050763,
        #         166.98894269, 157.78893941,  43.47861779, 158.31333177, 295.54599839,
        #          37.6091966 , 356.88930552,  22.54478043,  15.9960732 , 268.23297747,
        #           9.20597189,   6.07011578, 134.63084921, 274.60667748, 187.46528624,
        #         351.77271926,  78.59676186, 220.92052247, 187.97541604,  28.06002344,
        #          51.02182255, 295.11586412,  36.27931519,  84.87520623, 278.947349  ,
        #           6.78491531, 290.89290687, 163.54209697, 151.22641776, 161.35266334,
        #         286.26807052,  21.4312509 , 284.16607238, 221.176562  , 294.61080483,
        #         314.85773459, 346.96511185,  76.23959438,  24.6484304 ,  56.75380329,
        #         108.63943527, 294.62756515,  19.90284617, 105.39343508,  79.01729023,
        #         189.77500315, 186.27800503,  37.67171811,   5.51231152, 194.42744789,
        #          50.66166193,  94.20132964, 263.40082018, 109.69726426, 309.30485056,
        #         261.91945398,  84.31767056,  73.73963685, 355.80392719, 133.70609754,
        #         212.85807172,  17.02932275, 317.57570281,  14.95180697, 330.66188406,
        #          14.4546499 ,  45.9156886 , 330.16833049,  37.03475425,  90.58492179]
        self._logger.info('Created tasks with delay {0}'.format(delay))
        for i in range(num):
            ctask = CompositeTask(self._timer.getTime() + delay[i])
            ctask.add(Task(self._timer.getTime() + due, None, src[i], priority=np.random.randint(20)))
            ctask.add(Task(self._timer.getTime() + 3*due, src[i], dst[i], priority=np.random.randint(20)))
            ctask.add(Task(self._timer.getTime() + 6*due, dst[i], src[i], priority=np.random.randint(20)))
            #print('task from {0} to {1} with delay {2} generated'.format(src, dst[i], delay[i]))
            self._tcs.addCompositeTask(ctask)
        #print('tasks generated')
    
    def getCommunicationChannel(self):
        """
        获取通信信道。
        :return: 当前时间（整数类型）。
        """
        return self._comm_channel

    def getTransportationControlSystem(self):
        """
        获取任务分发器。
        :return: 任务分发器（Dispatcher对象）。
        """
        return self._tcs
        
    def getObstaclePool(self):
        return self._obstacle_pool

    def step(self, dt=None):
        if dt is None:
            dt = self._dt
        
        robot_num = self._robot_pool.getRobotsNum()
        #print('robots num', robot_num)
        for i in range(robot_num):
            self._robot_pool.getRobot(i).step(self._dt)
            self._traj.append(i, self._timer.getTime() + self._dt)
            self._traj.append(i, self._robot_pool.getRobot(i).getPos().getX())
            self._traj.append(i, self._robot_pool.getRobot(i).getPos().getY())
            
        self._tcs.process(dt)
        
        #if self._timer.getTime() > 840.15 and self._timer.getTime() < 840.25:
        #    self._traj.save()
        #    return True
        if self._tcs.allTasksCompleted(creator='user'):
            self._logger.info('All Tasks Completed at time {0:.2f}'.format(self._timer.getTime()))
            return True
        self._timer.updateTime(self._dt)
        return False
        
    def run(self):
        """
        仿真该虚拟的“世界”。
        """
        ii = 0
        while True:
            #print 'time {0}'.format(self._time)
            if ii == 0:
                #ctask = CompositeTask(self._timer.getTime() + 0.5)
                #ctask.add(Task(self._timer.getTime() + 10, '42', '49', priority=0))
                #self._tcs.addCompositeTask(ctask)
                
                #ctask = CompositeTask(self._timer.getTime()+0.5)
                #ctask.add(Task(self._timer.getTime() + 10, '43', '45', priority=1))
                #self._tcs.addCompositeTask(ctask)
                
                #ctask = CompositeTask(self._timer.getTime()+0.5)
                #ctask.add(Task(self._timer.getTime() + 10, '48', '46', priority=1))
                #self._tcs.addCompositeTask(ctask)
                
                
                #obstacle = Obstacle(self._timer, self._map, exist_time=0, disappear_time=15, edge=('18', '53'), division=0.5)
                #self._obstacle_pool.add(obstacle)
                
                self.addRandomTasks(self._task_num)
                
            if self.step():
                break
            ii += 1
        self._traj.save()


if __name__ == '__main__':
    import sys
    robot_num = [8]
    task_num = [40]
    for tn in task_num:
        for n in robot_num:
            for i in range(1):
                print('Robot num', n, 'task num', tn, 'sim', i)
                #sys.stdout = open('result/robot{0}_task{1}-{2}'.format(n, tn, i), 'w')
                world = SimWorld('config.ini', robot_num=n, task_num=tn)
                world.run()
                sys.stdout.flush()
